#include <chrono>
#include <map>
#include <memory>
#include <mutex>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

namespace multi_launch_demo
{

class ComponentMonitor : public rclcpp::Node
{
   public:
    ComponentMonitor(const rclcpp::NodeOptions &options) : Node("component_monitor", options)
    {
        // 声明参数
        this->declare_parameter("monitoring_interval_ms", 3000);
        this->declare_parameter("alert_threshold", 5);

        // 获取参数
        monitoring_interval_ms_ = this->get_parameter("monitoring_interval_ms").as_int();
        alert_threshold_ = this->get_parameter("alert_threshold").as_int();

        // 创建订阅者 - 监控多个主题
        raw_data_subscription_ = this->create_subscription<std_msgs::msg::String>(
            "raw_data", 10,
            [this](const std_msgs::msg::String::SharedPtr msg) { this->monitor_message("raw_data", msg); });

        processed_data_subscription_ = this->create_subscription<std_msgs::msg::String>(
            "processed_data", 10,
            [this](const std_msgs::msg::String::SharedPtr msg) { this->monitor_message("processed_data", msg); });

        aggregated_data_subscription_ = this->create_subscription<std_msgs::msg::String>(
            "aggregated_data", 10,
            [this](const std_msgs::msg::String::SharedPtr msg) { this->monitor_message("aggregated_data", msg); });

        // 创建发布者 - 发布监控报告
        monitor_publisher_ = this->create_publisher<std_msgs::msg::String>("monitor_report", 10);

        // 创建定时器 - 定期发布监控报告
        monitor_timer_ = this->create_wall_timer(std::chrono::milliseconds(monitoring_interval_ms_),
                                                 [this]() -> void { this->publish_monitor_report(); });

        RCLCPP_INFO(this->get_logger(), "Component Monitor started - Interval: %dms, Alert threshold: %d",
                    monitoring_interval_ms_, alert_threshold_);
    }

   private:
    void monitor_message(const std::string &topic_name, const std_msgs::msg::String::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(stats_mutex_);

        // 更新统计信息
        topic_stats_[topic_name].message_count++;
        topic_stats_[topic_name].last_message_time = this->get_clock()->now();
        topic_stats_[topic_name].last_message_size = msg->data.length();

        // 检查是否需要告警
        if (topic_stats_[topic_name].message_count > alert_threshold_)
        {
            RCLCPP_WARN(this->get_logger(), "High message count on topic '%s': %d messages", topic_name.c_str(),
                        topic_stats_[topic_name].message_count);
        }

        RCLCPP_DEBUG(this->get_logger(), "Monitored message on '%s': size=%zu, count=%d", topic_name.c_str(),
                     msg->data.length(), topic_stats_[topic_name].message_count);
    }

    void publish_monitor_report()
    {
        std::lock_guard<std::mutex> lock(stats_mutex_);

        auto report_msg = std_msgs::msg::String();
        report_msg.data = "[MONITOR REPORT] ";

        // 生成监控报告
        for (const auto &[topic_name, stats] : topic_stats_)
        {
            if (!report_msg.data.empty() && report_msg.data.back() != ' ')
            {
                report_msg.data += " | ";
            }
            report_msg.data += topic_name + ": " + std::to_string(stats.message_count) + " msgs";
        }

        // 添加时间戳
        auto now = this->get_clock()->now();
        report_msg.data += " @ " + std::to_string(now.nanoseconds());

        // 发布监控报告
        monitor_publisher_->publish(report_msg);

        RCLCPP_INFO(this->get_logger(), "Published monitor report: %s", report_msg.data.c_str());

        // 重置统计信息
        for (auto &[topic_name, stats] : topic_stats_)
        {
            stats.message_count = 0;
        }
    }

    struct TopicStats
    {
        int message_count = 0;
        rclcpp::Time last_message_time;
        size_t last_message_size = 0;
    };

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr raw_data_subscription_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr processed_data_subscription_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr aggregated_data_subscription_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr monitor_publisher_;
    rclcpp::TimerBase::SharedPtr monitor_timer_;

    std::map<std::string, TopicStats> topic_stats_;
    std::mutex stats_mutex_;

    int monitoring_interval_ms_;
    int alert_threshold_;
};

}  // namespace multi_launch_demo

RCLCPP_COMPONENTS_REGISTER_NODE(multi_launch_demo::ComponentMonitor)
